package com.hitqz.robot.biz.business.util;

import com.hitqz.robot.api.common.entity.MapAreaPos;
import com.hitqz.robot.biz.business.model.Line;
import com.hitqz.robot.biz.business.model.Point;
import lombok.extern.slf4j.Slf4j;
import org.locationtech.jts.geom.Coordinate;
import org.locationtech.jts.geom.GeometryFactory;
import org.locationtech.jts.geom.Polygon;

import java.math.BigDecimal;
import java.util.List;

/**
 * 计算工具类
 * @author xupkun
 * @date 2024/6/9
 */
@Slf4j
public class CalculateUtil {

    private static final GeometryFactory geometryFactory = new GeometryFactory();

    /**
     * 点到线的距离
     * @param line
     * @param x
     * @param y
     * @return
     */
    public static double getLine2PointDistance(Line line, double x, double y) {
        double distance = Math.abs(line.getA() * x + line.getB() * y + line.getC()) /
                Math.sqrt(Math.pow(line.getA(), 2) + Math.pow(line.getB(), 2));
        //log.info("点到直线的距离 直线方程{}-{}-{}, 点坐标{},{}, 距离 {}", line.getA(), line.getB(), line.getC(), x, y, distance );
        return distance;
    }

    /**
     * 点是否在线上
     * @param line
     * @param x
     * @param y
     * @return
     */

    public static boolean isOnLine(Line line, double x, double y) {
        if (x < Math.min(line.getStartPos().getX(), line.getEndPos().getX())
                || x > Math.max(line.getStartPos().getX(), line.getEndPos().getX())
        ) {
            return false;
        }
        if ( (y < Math.min(line.getStartPos().getY(), line.getEndPos().getY())) ||
                (y > Math.max(line.getStartPos().getY(), line.getEndPos().getY()))) {
            return false;
        }
        double distance = CalculateUtil.getLine2PointDistance(line, x, y);
        // 一个车的长度
        log.info("Line " + distance);
        return distance <= 2;
    }


    /**
     * 得到垂足坐标
     * @param line
     * @param x
     * @param y
     * @return
     */
    public static Point getFoot(Line line, double x, double y){
        double dx=line.getStartPos().getX()- line.getEndPos().getX();
        double dy= line.getStartPos().getY()- line.getEndPos().getY();
        double u=(x-line.getStartPos().getX())*dx+(y-line.getStartPos().getX())*dy;
        u/=dx*dx+dy*dy;
        Point point = new Point();
        point.setX(new BigDecimal(line.getStartPos().getX()+u*dx).setScale(3,BigDecimal.ROUND_UP).floatValue());
        point.setY(new BigDecimal(line.getStartPos().getY()+u*dy).setScale(3,BigDecimal.ROUND_UP).floatValue());
        double d=Math.abs((line.getStartPos().getX()-line.getEndPos().getX())*(line.getStartPos().getX()-line.getEndPos().getX())
                +(line.getStartPos().getY()-line.getEndPos().getY())*(line.getStartPos().getX()-line.getEndPos().getY()));
        double d1=Math.abs((line.getStartPos().getX()-point.getX())*(line.getStartPos().getX()-point.getX())
                +(line.getStartPos().getY()-point.getY())*(line.getStartPos().getX()-point.getY()));
        double d2=Math.abs((line.getEndPos().getX()-point.getX())*(line.getEndPos().getX()-point.getX())
                +(line.getEndPos().getY()-point.getY())*(line.getEndPos().getX()-point.getY()));

        if(d1>d||d2>d){
            if (d1>d2)  {
                return line.getEndPos();
            }
            else {
                return line.getStartPos();
            }
        }
        return point;
    }



    public static boolean isPtInPoly(Point point, List<Point> pts) {
        int N = pts.size();
        boolean boundOrVertex = true; //如果点位于多边形的顶点或边上，也算做点在多边形内，直接返回true  
        int intersectCount = 0;//cross points count of x   
        double precision = 2e-10; //浮点类型计算时候与0比较时候的容差  
        Point p1, p2;//neighbour bound vertices  
        Point p = point; //当前点
        p1 = pts.get(0);//left vertex          
        for(int i = 1; i <= N; ++i){//check all rays              
            if(p.equals(p1)){
                return boundOrVertex;//p is an vertex  
            }

            p2 = pts.get(i % N);//right vertex              
            if(p.getX() < Math.min(p1.getX(), p2.getX()) || p.getX() > Math.max(p1.getX(), p2.getX())){//ray is outside of our interests
                p1 = p2;
                continue;//next ray left point  
            }

            if(p.getX() > Math.min(p1.getX(), p2.getX()) && p.getX() < Math.max(p1.getX(), p2.getX())){//横坐标 内  ray is crossing over by the algorithm (common part of)
                if(p.getY() <= Math.max(p1.getY(), p2.getY())){//y下  x is before of ray
                    if(p1.getX() == p2.getX() && p.getY() >= Math.min(p1.getY(), p2.getY())){//overlies on a horizontal ray  垂线
                        return boundOrVertex;
                    }

                    if(p1.getY() == p2.getY()){//水平线 ray is vertical
                        if(p1.getY() == p.getY()){//水平线内 overlies on a vertical ray
                            return boundOrVertex;
                        }else{//before ray  
                            ++intersectCount;  //交点在上方
                        }
                    }else{//cross point on the left side                          
                        double xinters = (p.getX() - p1.getX()) * (p2.getY() - p1.getY()) / (p2.getX() - p1.getX()) + p1.getY();//两点式化简，交点y坐标 cross point of y
                        if(Math.abs(p.getY() - xinters) < precision){//== 0  在线上  overlies on a ray
                            return boundOrVertex;
                        }

                        if(p.getY() < xinters){//before ray
                            ++intersectCount;  //交点在上方
                        }
                    }
                }
            }else{//special case when ray is crossing through the vertex                  
                if(p.getX() == p2.getX() && p.getY() <= p2.getY()){//p crossing over p2
                    Point p3 = pts.get((i+1) % N); //next vertex                      
                    if(p.getX() >= Math.min(p1.getX(), p3.getX()) && p.getX() <= Math.max(p1.getX(), p3.getX())){//p.x lies between p1.x & p3.x
                        ++intersectCount;
                    }else{
                        intersectCount += 2;
                    }
                }
            }
            p1 = p2;//next ray left point  
        }

        if(intersectCount % 2 == 0){//偶数在多边形外  
            return false;
        } else { //奇数在多边形内  
            return true;
        }
    }

    public static Polygon getRectangleVertices(double cx, double cy, double length, double width) {
        if (length <= 0 || width <= 0) {
            throw new IllegalArgumentException("Length and width must be positive.");
        }

        double halfLength = length / 2.0;
        double halfWidth = width / 2.0;

        Coordinate[] coordinates = new Coordinate[5];
        coordinates[0] = new Coordinate(cx - halfLength, cy + halfWidth, 0);
        coordinates[1] = new Coordinate(cx + halfLength, cy + halfWidth, 0);
        coordinates[2] = new Coordinate(cx + halfLength, cy - halfWidth, 0);
        coordinates[3] = new Coordinate(cx - halfLength, cy - halfWidth, 0);
        coordinates[4] = new Coordinate(cx - halfLength, cy + halfWidth,0);

        try {
            return geometryFactory.createPolygon(coordinates);
        } catch (Exception e) {
            // 更详细的异常处理
            System.err.println("Error creating polygon: " + e.getMessage());
            throw new RuntimeException("Failed to create polygon", e);
        }
    }


    public static Polygon createAreaPolygon(List<MapAreaPos> mapAreaPos) {
        if (mapAreaPos == null || mapAreaPos.isEmpty()) {
            throw new IllegalArgumentException("The list of positions cannot be null or empty.");
        }
        Coordinate[] coordinates = new Coordinate[mapAreaPos.size()+1];
        for (int i = 0; i < coordinates.length-1; i++) {
            MapAreaPos pos = mapAreaPos.get(i);
            coordinates[i] = new Coordinate(pos.getPosx(), pos.getPosy(), 0);
        }
        coordinates[mapAreaPos.size()] = new Coordinate(coordinates[0].x, coordinates[0].y, coordinates[0].z);
        try {
            return geometryFactory.createPolygon(coordinates);
        } catch (Exception e) {
            return null;
        }
    }








}
